#include <ipa_room_exploration/boustrophedon_explorator.h>

//#define DEBUG_VISUALIZATION

// Constructor
BoustrophedonExplorer::BoustrophedonExplorer() {}

// Function that creates a room exploration path for the given map, by using the morse cellular decomposition method proposed in
// 使用morse细胞分解方法为给定地图创建一个房间探索路径的函数
// "H. Choset, E. Acar, A. A. Rizzi and J. Luntz,
// "Exact cellular decompositions in terms of critical points of Morse functions," Robotics and Automation, 2000. Proceedings.
// ICRA '00. IEEE International Conference on, San Francisco, CA, 2000, pp. 2270-2277 vol.3."
//
// This method takes the given map and separates it into several cells. Each cell is obstacle free and so allows an
// easier path planning. For each cell then a boustrophedon path is planned, which goes up, down and parallel to the
// upper and lower boundaries of the cell, see the referenced paper for details. This function does the following steps:
// I.	Using the Sobel operator the direction of the gradient at each pixel is computed. Using this information, the direction is
//		found that suits best for calculating the cells, i.e. such that longer cells occur, and the map is rotated in this manner.
//		This allows to use the algorithm as it was and in the last step, the found path points simply will be transformed back to the
//		original orientation.
// II.	Sweep a slice (a morse function) trough the given map and check for connectivity of this line,
//		i.e. how many connected segments there are. If the connectivity increases, i.e. more segments appear,
//		an IN event occurs that opens new separate cells, if it decreases, i.e. segments merge, an OUT event occurs that
//		merges two cells together. If an event occurs, the algorithm checks along the current line for critical points,
//		that are points that trigger the events. From these the boundary of the cells are drawn, starting from the CP
//		and going left/right until a black pixel is hit.
// III.	After all cells have been determined by the sweeping slice, the algorithm finds these by using cv::findContours().
//		This gives a set of points for each cell, that are used to create a generalizedPolygon out of each cell.
// IV.	After all polygons have been created, plan the path trough all of them for the field of view s.t. the whole area
//		is covered. To do so, first a global path trough all cells is generated, using the traveling salesmen problem
//		formulation. This produces an optimal visiting order of the cells. Next for each cell a boustrophedon path is
//		determined, which goes back and forth trough the cell and between the horizontal paths along the boundaries of
//		the cell, what ensures that the whole area of the cell is covered. For each cell the longest edge is found and it is transformed
//		s.t. this edge lies horizontal to the x-direction. This produces longer but fewer edges, what improves the path for small but long
//		cells. The startpoint of the cell-path is determined by the endpoint of the previous cell, s.t. the distance between two
//		cell-paths is minimized. The cell-path is determined in the rotated manner, so in a last step, the cell-path is transformed to
//		the originally transformed cell and after that inserted in the global path.
// V.	The previous step produces a path for the field of view. If wanted this path gets mapped to the robot path s.t.
//		the field of view follows the wanted path. To do so simply a vector transformation is applied. If the computed robot
//		pose is not in the free space, another accessible point is generated by finding it on the radius around the field of view (fov)
//		middlepoint s.t. the distance to the last robot position is minimized. If this is not wanted one has to set the
//		corresponding Boolean to false (shows that the path planning should be done for the robot footprint).
// room_map = expects to receive the original, not inflated room map
void BoustrophedonExplorer::getExplorationPath(const cv::Mat &room_map, std::vector<geometry_msgs::Pose2D> &path,
                                               const float map_resolution, const cv::Point starting_position,
                                               const cv::Point2d map_origin,
                                               const double grid_spacing_in_pixel, const double grid_obstacle_offset,
                                               const double path_eps, const int cell_visiting_order,
                                               const bool plan_for_footprint,
                                               const Eigen::Matrix<float, 2, 1> robot_to_fov_vector,
                                               const double min_cell_area, const int max_deviation_from_track)
{
    ROS_INFO("Planning the boustrophedon path trough the room.");
    const int grid_spacing_as_int = (int) std::floor(grid_spacing_in_pixel); // convert fov-radius to int
    const int half_grid_spacing_as_int = (int) std::floor(0.5 * grid_spacing_in_pixel); // convert fov-radius to int
    const int min_cell_width = half_grid_spacing_as_int + 2. * grid_obstacle_offset / map_resolution;

    // I. Find the main directions of the map and rotate it in this manner. 找到地图的主要方向并以这种方式旋转。
    // II. Sweep a slice trough the map and mark the found cell boundaries. 在地图上扫一扫，并标出找到的单元格边界.
    // III. Find the separated cells. 找到分离的单元格。
    cv::Mat R;
    cv::Rect bbox;
    cv::Mat rotated_room_map;
    std::vector<GeneralizedPolygon> cell_polygons;
    std::vector<cv::Point> polygon_centers;
    // 该函数实现两个功能: 第一个是对地图进行长短边计算，计算得出最佳的旋转矩阵，并将地图进行旋转。
    //                  第二则是调用computeCellDecomposition()函数进行区间分割，具体的分区实现逻辑后续再讲，
    // 该函数返回每一个区间的多边形点数组cell_polygons以及每一个区间的中心点数组polygon_centers。
    // 返回的cell_polygons用来确定区间在地图上的位置，polygon_centers则是用来确定区间跟区间之间的距离。
    computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R, bbox,
                                         rotated_room_map, cell_polygons, polygon_centers);
    // 至此，就完成了区间的分割合并操作，并将结果输入给下一步做具体的遍历顺序计算以及弓字形覆盖路径生成。
    // does not work so well: findBestCellDecomposition(room_map, map_resolution, min_cell_area, R, bbox, rotated_room_map, cell_polygons, polygon_centers);
    ROS_INFO("Found the cells in the given map.");
    // *********************** IV. Determine the cell paths.确定单元格路径。 ***********************
    // determine the start cell that contains the start position
    std::vector<cv::Point> starting_point_vector(1, starting_position); // opencv syntax
    cv::transform(starting_point_vector, starting_point_vector, R);
    const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell
#ifdef DEBUG_VISUALIZATION
    //	testing
    cv::Mat center_map = rotated_room_map.clone();
    for(auto & cell_polygon : cell_polygons)
        cv::circle(center_map, cell_polygon.getCenter(), 2, cv::Scalar(127), cv::FILLED);
    cv::circle(center_map, starting_position, 4, cv::Scalar(100), cv::FILLED);
    cv::imshow("centers", center_map);
    cv::waitKey(0);
#endif
    int start_cell_index = 0;
    for (auto cell = cell_polygons.begin(); cell != cell_polygons.end(); ++cell)
    {
        if (cv::pointPolygonTest(cell->getVertices(), rotated_starting_point, false) >= 0)
            start_cell_index = cell - cell_polygons.begin();
    }
    // determine the visiting order of the cells
    std::vector<int> optimal_order;
    // 第一种是通过TSP计算得出区间遍历顺序，
    if (cell_visiting_order == OPTIMAL_TSP)
    {
        // determine the optimal visiting order of the cells
        //	ConcordeTSPSolver tsp_solver;
        //	std::vector<int> optimal_order = tsp_solver.solveConcordeTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
        GeneticTSPSolver tsp_solver;
        // 计算各区间的遍历顺序，本质上可以看作是一个TSP问题（即旅行商问题），只不过不要求回到起点位置，故可以用求解TSP的方式求出各个区间的遍历顺序。
        // 求解TSP的方式分为两类：一类是求出最优解，另一种则是求出近似解。
        // 由于TSP属于NP完全问题，而且一般应用在智能扫地机上面的算法板算力并不高，
        // 故一般在实际应用中，不会要求求出最优解，能得到一个相对较合理的近似解即可。
        // ROS默认使用的计算TSP遍历顺序的算法是遗传算法，且会先将地图缩放0.25倍后进行计算
        optimal_order = tsp_solver.solveGeneticTSP(
                rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
        if (optimal_order.size() != polygon_centers.size())
        {
            std::cout << "==========> Genetic TSP failed with 25% resolution, falling back to 100%. <=========="
                      << std::endl;
            optimal_order = tsp_solver.solveGeneticTSP(
                    rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
        }
    }
    // 第二种则是简单的通过polygon_centers的Y坐标从小到大排序。
    else if (cell_visiting_order == LEFT_TO_RIGHT)
    {
        // we are using an alternative ordering here, which visits the cells in a more obvious fashion to the human observer (though it is not optimal)
        std::multimap<int, int> y_coordinate_ordering;        // <y-coordinate of room centers, index>
        for (size_t i = 0; i < polygon_centers.size(); ++i)
            y_coordinate_ordering.insert(std::pair<int, int>(polygon_centers[i].y, (int) i));
        for (auto &it : y_coordinate_ordering) optimal_order.push_back(it.second);
    }
    else
    {
        std::cout << "Error: BoustrophedonExplorer::getExplorationPath: The specified cell_visiting_order="
                  << cell_visiting_order << " is invalid." << std::endl;
        return;
    }
    // go trough the cells [in optimal visiting order] and determine the boustrophedon paths
    ROS_INFO("Starting to get the paths for each cell, number of cells: %d", (int) cell_polygons.size());
    std::cout << "Boustrophedon grid_spacing_as_int=" << grid_spacing_as_int << std::endl;
    // point that keeps track of the last point after the boustrophedon path in each cell
    cv::Point robot_pos = rotated_starting_point;
    // this is the trajectory of centers of the robot footprint or the field of view
    std::vector<cv::Point2f> fov_middlepoint_path;
    // 上述操作完成后，就得到各个子区间以及区间的遍历顺序，接下来则是要计算各区间内的弓字形覆盖路线
    //  rotated_room_map  为经过矩阵旋转后的地图，
    //  optimal_order     为生成的区间遍历顺序，
    //  fov_middlepoint_path 为生成的总的覆盖直线点，
    //  robot_pos            为机器人坐标（注意该坐标在每次计算后都会发生改变），
    //  grid_spacing_as_int  为覆盖直线的宽度，
    //  path_eps             控制生成路径点的疏密。
    for (size_t cell = 0; cell < cell_polygons.size(); ++cell)
    {
        computeBoustrophedonPath(rotated_room_map, map_resolution, cell_polygons[optimal_order[cell]],
                                 fov_middlepoint_path, robot_pos, grid_spacing_as_int, half_grid_spacing_as_int, path_eps,
                                 max_deviation_from_track, grid_obstacle_offset / map_resolution);
    }
    // transform the calculated path back to the originally rotated map and create poses with an angle
    RoomRotator room_rotation;
    // this is the trajectory of poses of the robot footprint or the field of view, in [pixels]
    std::vector<geometry_msgs::Pose2D> fov_poses;
    // 但由于是在旋转后的地图上进行计算得出的，故还需要通过旋转矩阵将坐标点仿射变换到原图
    room_rotation.transformPathBackToOriginalRotation(fov_middlepoint_path, fov_poses, R);
#ifdef DEBUG_VISUALIZATION
    std::cout << "printing path" << std::endl;
    cv::Mat room_map_path = room_map.clone();
    cv::circle(room_map_path, starting_position, 3, cv::Scalar(160), cv::FILLED);
    for(size_t i=0; i<fov_poses.size()-1; ++i)
    {
        cv::circle(room_map_path, cv::Point(cvRound(fov_poses[i].x), cvRound(fov_poses[i].y)), 1, cv::Scalar(200), cv::FILLED);
        cv::line(room_map_path, cv::Point(cvRound(fov_poses[i].x), cvRound(fov_poses[i].y)), cv::Point(cvRound(fov_poses[i+1].x), cvRound(fov_poses[i+1].y)), cv::Scalar(100), 1);
    }
    cv::circle(room_map_path, cv::Point(cvRound(fov_poses.back().x), cvRound(fov_poses.back().y)), 1, cv::Scalar(200), cv::FILLED);
    //	for(size_t i=0; i<optimal_order.size()-1; ++i)
    //		cv::line(room_map_path, polygon_centers[optimal_order[i]], polygon_centers[optimal_order[i+1]], cv::Scalar(100), 1);
    cv::imshow("room_map_path_intermediate", room_map_path);
    if (plan_for_footprint) cv::waitKey();
#endif
    ROS_INFO("Found the cell paths.");

    // if the path should be planned for the robot footprint create the path and return here
    // 如果路径应该规划为机器人的足迹创建路径并返回到这里
    if (plan_for_footprint)
    {
        for (auto &fov_pose : fov_poses)
        {
            geometry_msgs::Pose2D current_pose;
            current_pose.x = (fov_pose.x * map_resolution) + map_origin.x;
            current_pose.y = (fov_pose.y * map_resolution) + map_origin.y;
            current_pose.theta = fov_pose.theta;
            path.push_back(current_pose);
        }
        return;
    }
    // *********************** V. Get the robot path out of the fov path. 把机器人的路径移出视野。 ***********************
    // go trough all computed fov poses and compute the corresponding robot pose
    // 得到的坐标点是图像上的坐标点，故最后需要转换坐标系。
    ROS_INFO("Starting to map from field of view pose to robot pose");
    cv::Point robot_starting_position =
            !fov_poses.empty() ? cv::Point(cvRound(fov_poses[0].x), cvRound(fov_poses[0].y)) : starting_position;
    cv::Mat inflated_room_map;
    cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1), half_grid_spacing_as_int);
    // 进行坐标转换的同时，把一些贴近障碍物的坐标点给去掉。
    mapPath(inflated_room_map, path, fov_poses, robot_to_fov_vector,
            map_resolution, map_origin, robot_starting_position);
#ifdef DEBUG_VISUALIZATION
    // testing
    std::cout << "printing path" << std::endl;
    cv::Mat fov_path_map = room_map.clone();
    if (path.size() > 0)
        cv::circle(fov_path_map, cv::Point((path[0].x-map_origin.x)/map_resolution, (path[0].y-map_origin.y)/map_resolution), 2, cv::Scalar(150), cv::FILLED);
    for(size_t i=1; i<path.size(); ++i)
    {
        cv::Point p1((path[i-1].x-map_origin.x)/map_resolution, (path[i-1].y-map_origin.y)/map_resolution);
        cv::Point p2((path[i].x-map_origin.x)/map_resolution, (path[i].y-map_origin.y)/map_resolution);
        cv::circle(fov_path_map, p2, 1, cv::Scalar(200), cv::FILLED);
        cv::line(fov_path_map, p1, p2, cv::Scalar(100), 1);
        cv::Point p3(p2.x+5*cos(path[i].theta), p2.y+5*sin(path[i].theta));
        cv::line(fov_path_map, p2, p3, cv::Scalar(50), 1);
//		cv::imshow("cell path", fov_path_map);
//		cv::waitKey();
    }
    cv::imshow("room_map_path_final", fov_path_map);
    cv::waitKey();
#endif
}


void BoustrophedonExplorer::findBestCellDecomposition(const cv::Mat &room_map, const float map_resolution,
                                                      const double min_cell_area,
                                                      const int min_cell_width, cv::Mat &R, cv::Rect &bbox,
                                                      cv::Mat &rotated_room_map,
                                                      std::vector<GeneralizedPolygon> &cell_polygons,
                                                      std::vector<cv::Point> &polygon_centers)
{
    // *********************** I. Find the main directions of the map and rotate it in this manner. ***********************
    // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
    // *********************** III. Find the separated cells. ***********************
    cv::Mat R_1, R_2;
    cv::Rect bbox_1, bbox_2;
    cv::Mat rotated_room_map_1, rotated_room_map_2;
    std::vector<GeneralizedPolygon> cell_polygons_1, cell_polygons_2;
    std::vector<cv::Point> polygon_centers_1, polygon_centers_2;
    computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R_1, bbox_1,
                                         rotated_room_map_1, cell_polygons_1, polygon_centers_1);
    computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 90. / 180. * CV_PI,
                                         R_2, bbox_2, rotated_room_map_2, cell_polygons_2, polygon_centers_2);

    // select the cell decomposition with good axis alignment which produces less cells
    // 选择具有良好轴对齐的单元格分解，生成较少的单元格
    if (cell_polygons_1.size() <= cell_polygons_2.size())
    {
        R = R_1;
        bbox = bbox_1;
        rotated_room_map = rotated_room_map_1;
        cell_polygons = cell_polygons_1;
        polygon_centers = polygon_centers_1;
    }
    else
    {
        R = R_2;
        bbox = bbox_2;
        rotated_room_map = rotated_room_map_2;
        cell_polygons = cell_polygons_2;
        polygon_centers = polygon_centers_2;
    }
}

void BoustrophedonExplorer::computeCellDecompositionWithRotation(
        const cv::Mat &room_map, const float map_resolution, const double min_cell_area,
        const int min_cell_width, const double rotation_offset, cv::Mat &R, cv::Rect &bbox, cv::Mat &rotated_room_map,
        std::vector<GeneralizedPolygon> &cell_polygons, std::vector<cv::Point> &polygon_centers)
{
    // *********************** I. Find the main directions of the map and rotate it in this manner. ***********************
    // *********************** 1 .找到地图的主要方向，这样旋转地图。 ***********************
    RoomRotator room_rotation;
    room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset);
    room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);
#ifdef DEBUG_VISUALIZATION
    // testing
    cv::Mat room_map_disp = room_map.clone();
//    cv::circle(room_map_disp, starting_position, 3, cv::Scalar(160), cv::FILLED);
    cv::imshow("room_map", room_map_disp);
    std::vector<cv::Point> tester;
    tester.emplace_back(10, 10);
    cv::Mat tester_map = room_map.clone();
    cv::circle(tester_map, tester[0], 3, cv::Scalar(127), cv::FILLED);
    cv::transform(tester, tester, R);
    cv::circle(rotated_room_map, tester[0], 3, cv::Scalar(127), cv::FILLED);
    cv::imshow("original", tester_map);
    cv::imshow("rotated_im", rotated_room_map);
//    cv::waitKey();
#endif

    // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
    // *********************** II. 通过地图扫描切片并标记找到的cell边界。 ***********************
    // *********************** III. Find the separated cells. 找到分离的单元格。***********************
    computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers);
}

// room_map 是经过预处理以及最佳角度旋转后的栅格地图，该地图是一张二值图，0代表障碍物，255代表可通行区域；
// map_resolution 是栅格地图的分辨率；
// min_cell_area 用来过滤小面积区域；
// min_cell_width 用来过滤窄区域；
// cell_polygons 保存返回的分区结果，生成的区间以多边形点的形式保存；
// polygon_centers 保存每一个分区的中心点坐标，用来计算区间之间的距离。
// 该函数的主要分为两个步骤：第一步是使用牛耕法将地图分割为若干个子区间；第二步则是对第一步分割好的区间做一个合并操作，将小面积区间合并到相邻面积最大的区间。
// 牛耕法会逐行（或逐列）计算该行（或该列）被障碍物分为几段， 根据前后连续两行（或两列）的段数，会触发不同的处理逻辑：
//1.当前段数比上一个的段数多，则触发IN事件处理。
//2.当前段数比上一个的段数少，则触发OUT事件处理。
//3.当前段跟上一个的段数一样，但不连续，则同样当IN事件处理。
void BoustrophedonExplorer::computeCellDecomposition(
        const cv::Mat &room_map, const float map_resolution, const double min_cell_area,
        const int min_cell_width, std::vector<GeneralizedPolygon> &cell_polygons,
        std::vector<cv::Point> &polygon_centers)
{
    // *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
    // 通过地图扫描切片并标记找到的细胞边界。
    // create a map copy to mark the cell boundaries
    // 创建地图副本以标记单元格边界
    cv::Mat cell_map = room_map.clone();
    // find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
    // 找到白色像素出现的最小y值，设置初始y值并找到初始分段数
    size_t y_start = 0;
    bool found = false, obstacle = false;
    int previous_number_of_segments = 0;
    // keep track of the end points of obstacles 跟踪障碍物的终点
    std::vector<int> previous_obstacles_end_x;
    // 这段代码的功能是从上到下，从左到右，遍历地图数据，当找到房间的第一行时，记录下当时的Y坐标y_start，
    // 以及该行的分段数previous_number_of_segments和障碍点的X坐标列表previous_obstacles_end_x。为后续进一步做计算做好准备。
    //当输入的地图数据比较复杂时，可能会生成比较多的小区间，这时候就要对上一步生成的区间进行合并。
    for (size_t y = 0; y < room_map.rows; ++y)
    {
        for (size_t x = 0; x < room_map.cols; ++x)
        {
            // 白色
            if (!found && room_map.at<uchar>(y, x) == 255)
            {
                y_start = y;
                found = true;
            }
            else if (found && !obstacle && room_map.at<uchar>(y, x) == 0)
            {
                ++previous_number_of_segments;
                obstacle = true;
            }
            else if (found && obstacle && room_map.at<uchar>(y, x) == 255)
            {
                obstacle = false;
                previous_obstacles_end_x.push_back(x);
            }
        }
        if (found) break;
    }
    // sweep trough the map and detect critical points
    // 从y_start+1开始，因为我们知道y_start的段数,扫过地图和检测关键点
    for (size_t y = y_start + 1; y < room_map.rows; ++y)
    {
        // int计算当前切片上有多少段
        // int to count how many segments at the current slice are
        int number_of_segments = 0;
        std::vector<int> current_obstacles_start_x;
        std::vector<int> current_obstacles_end_x;
        // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event
        // Bool检查线条当前是否碰到障碍物，例如，并非所有黑色像素都触发事件
        bool obstacle_hit = false;
        // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel
        // Bool检查当前切片是否命中白色像素，从第一个白色像素开始切片
        bool hit_white_pixel = false;
        // count number of segments within this row
        // 统计本行中的段数、保存远离障碍物时第一个可达点
        for (size_t x = 0; x < room_map.cols; ++x)
        {
            if (!hit_white_pixel && room_map.at<uchar>(y, x) == 255) hit_white_pixel = true;
            else if (hit_white_pixel)
            {
                // check for obstacle
                if (!obstacle_hit && room_map.at<uchar>(y, x) == 0)
                {
                    ++number_of_segments;
                    obstacle_hit = true;
                    current_obstacles_start_x.push_back(x);
                }
                    // check for leaving obstacle
                else if (obstacle_hit && room_map.at<uchar>(y, x) == 255)
                {
                    obstacle_hit = false;
                    current_obstacles_end_x.push_back(x);
                }
            }
        }
        // reset hit_white_pixel to use this Boolean later
        // 重置击中到白色像素以稍后使用该布尔值
        hit_white_pixel = false;
        // if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
        // 如果段的数量没有变化，检查段的位置是否发生了变化，使得段之间有间隙
        bool segment_shift_detected = false;
        if (previous_number_of_segments == number_of_segments &&
            current_obstacles_start_x.size() == previous_obstacles_end_x.size() + 1)
        {
            for (size_t i = 0; i < previous_obstacles_end_x.size(); ++i)
                if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
                {
                    segment_shift_detected = true;
                    break;
                }
        }
        // check if number of segments has changed --> event occurred // IN event (or shift)
        if (previous_number_of_segments < number_of_segments || segment_shift_detected)
        {
            // check the current slice again for critical points
            // 再次检查当前切片是否有临界点
            for (int x = 0; x < room_map.cols; ++x)
            {
                if (!hit_white_pixel && room_map.at<uchar>(y, x) == 255) hit_white_pixel = true;
                else if (hit_white_pixel && room_map.at<uchar>(y, x) == 0)
                {
                    // check over black pixel for other black pixels, if none occur a critical point is found
                    // 检查其他黑色像素的黑色像素，如果没有发现一个临界点
                    bool critical_point = true;
                    for (int dx = -1; dx <= 1; ++dx)
                    {
                        if (room_map.at<uchar>(y - 1, std::max(0, std::min(x + dx, room_map.cols - 1))) == 0)
                            critical_point = false;
                    }
                    // if a critical point is found mark the separation,
                    // note that this algorithm goes left and right starting at the critical point until an obstacle is hit,
                    // because this prevents unnecessary cells behind other obstacles on the same y-value as the critical point
                    // 如果找到一个临界点，标记分离，
                    // 注意这个算法从临界点开始向左和向右移动，直到一个障碍物被击中，
                    // 因为这防止了不必要的细胞在其他障碍物后面，在相同的y值作为临界点
                    if (critical_point)
                    {
                        // to the left until a black pixel is hit 向左移动，直到碰到一个黑色像素
                        for (int dx = -1; x + dx >= 0; --dx)
                        {
                            uchar &val = cell_map.at<uchar>(y, x + dx);
                            uchar &val2 = cell_map.at<uchar>(y - 1, x + dx);
                            if (val == 255 && val2 == 255) val = BORDER_PIXEL_VALUE;
                            else if (val == 0) break;
                        }
                        // to the right until a black pixel is hit 向右移动，直到碰到一个黑色像素
                        for (int dx = 1; x + dx < room_map.cols; ++dx)
                        {
                            uchar &val = cell_map.at<uchar>(y, x + dx);
                            uchar &val2 = cell_map.at<uchar>(y - 1, x + dx);
                            if (val == 255 && val2 == 255) val = BORDER_PIXEL_VALUE;
                            else if (val == 0) break;
                        }
                    }
                }
            }
        }
        else if (previous_number_of_segments > number_of_segments) // OUT event
        {
            // check the previous slice again for critical points --> y-1
            for (int x = 0; x < room_map.cols; ++x)
            {
                if (room_map.at<uchar>(y - 1, x) == 255 && !hit_white_pixel) hit_white_pixel = true;
                else if (hit_white_pixel && room_map.at<uchar>(y - 1, x) == 0)
                {
                    // check over black pixel for other black pixels, if none occur a critical point is found
                    bool critical_point = true;
                    for (int dx = -1; dx <= 1; ++dx)
                        // check at side after obstacle  在障碍后检查
                        if (room_map.at<uchar>(y, std::max(0, std::min(x + dx, room_map.cols - 1))) == 0)
                            critical_point = false;
                    // if a critical point is found mark the separation, 
                    // note that this algorithm goes left and right starting at the critical point until an obstacle is hit, 
                    // because this prevents unnecessary cells behind other obstacles on the same y-value as the critical point
                    // 如果找到一个临界点，标记分离，
                    // 注意这个算法从临界点开始向左和向右移动，直到一个障碍物被击中，
                    // 因为这防止了不必要的细胞在其他障碍物后面，在相同的y值作为临界点
                    if (critical_point)
                    {
                        const int ym2 = std::max(0, (int) y - 2);
                        // to the left until a black pixel is hit
                        for (int dx = -1; x + dx >= 0; --dx)
                        {
                            uchar &val = cell_map.at<uchar>(y - 1, x + dx);
                            uchar &val2 = cell_map.at<uchar>(ym2, x + dx);
                            if (val == 255 && val2 == 255) val = BORDER_PIXEL_VALUE;
                            else if (val == 0) break;
                        }
                        // to the right until a black pixel is hit
                        for (int dx = 1; x + dx < room_map.cols; ++dx)
                        {
                            uchar &val = cell_map.at<uchar>(y - 1, x + dx);
                            uchar &val2 = cell_map.at<uchar>(ym2, x + dx);
                            if (val == 255 && val2 == 255) val = BORDER_PIXEL_VALUE;
                            else if (val == 0) break;
                        }
                    }
                }
            }
        }
        // save the found number of segments and the obstacle end points
        // 保存找到的段数和障碍终点
        previous_number_of_segments = number_of_segments;
        previous_obstacles_end_x = current_obstacles_end_x;
    }
#ifdef DEBUG_VISUALIZATION
    cv::imshow("cell_map", cell_map);
#endif
    // *********************** II.b) merge too small cells into bigger cells 将太小的单元格合并为更大的单元格 ***********************
    // 将太小的单元格合并为更大的单元格
    cv::Mat cell_map_labels;
    const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);
    // *********************** III. Find the separated cells. 找到分离的单元格。 ***********************
    std::vector<std::vector<cv::Point> > cells;
    for (int i = 1; i <= number_of_cells; ++i)
    {
        cv::Mat cell_copy(cell_map_labels == i);
        std::vector<std::vector<cv::Point> > cellsi;
#if CV_MAJOR_VERSION <= 3
        cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
#else
        cv::findContours(cell_copy, cellsi, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
#endif
        cells.insert(cells.end(), cellsi.begin(), cellsi.end());
    }
#ifdef DEBUG_VISUALIZATION
    // testing
    cv::Mat black_map = cv::Mat::zeros(cell_map.rows, cell_map.cols, cell_map.type());
    for (size_t i = 0; i < cells.size(); ++i) {
        cv::drawContours(black_map, cells, i, cv::Scalar(i * 10 + 20), cv::FILLED);
    }
    cv::imshow("contours", black_map);
//    cv::waitKey();
#endif
    // create generalized Polygons out of the contours to handle the cells
    // 创建广义多边形处理单元的轮廓
    for (size_t cell = 0; cell < cells.size(); ++cell)
    {
        GeneralizedPolygon current_cell(cells[cell], map_resolution);
//		std::cout << cell+1 << ": " << current_cell.getArea() << std::endl;
        if (current_cell.getArea() >= min_cell_area)
        {
            cell_polygons.push_back(current_cell);
            polygon_centers.push_back(current_cell.getCenter());
        }
        else
        {
            std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell + 1
                      << " with area=" << current_cell.getArea()
                      << ". This should only happen for small unconnected cells." << std::endl;
        }
    }
}

int BoustrophedonExplorer::mergeCells(cv::Mat &cell_map, cv::Mat &cell_map_labels, const double min_cell_area,
                                      const int min_cell_width)
{
    // label all cells 标记所有单元格
    //   --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels
    cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0);
    //   --> re-assign the cell borders with -1  用-1重新分配单元格边界
    for (int v = 0; v < cell_map_labels.rows; ++v)
        for (int u = 0; u < cell_map_labels.cols; ++u)
            if (cell_map_labels.at<int>(v, u) == BORDER_PIXEL_VALUE * 256)
                cell_map_labels.at<int>(v, u) = -1;
    //   --> flood fill cell regions with unique id labels 用唯一的id标签填充单元格区域
    // maps each cell label --> to the cell object
    std::map<int, boost::shared_ptr<BoustrophedonCell> > cell_index_mapping;
    int label_index = 1;
    for (int v = 0; v < cell_map_labels.rows; ++v)
    {
        for (int u = 0; u < cell_map_labels.cols; ++u)
        {
            // if the map has already received a label for that pixel --> skip
            // 如果地图已经接收到该像素的标签 ——> 跳过
            if (cell_map_labels.at<int>(v, u) != 65280) continue;
            // fill each cell with a unique id 用唯一的id填充每个单元格
            cv::Rect bounding_box;
            const double area = cv::floodFill(cell_map_labels, cv::Point(u, v), label_index, &bounding_box, 0, 0, 4);
            cell_index_mapping[label_index] = boost::shared_ptr<BoustrophedonCell>(
                    new BoustrophedonCell(label_index, area, bounding_box));
            label_index++;
            if (label_index == INT_MAX)
                std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl;
        }
    }
    std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index - 1
              << " cells before merging." << std::endl;
    // determine the neighborhood relationships between all cells
    // 确定所有单元之间的邻域关系
    for (int v = 1; v < cell_map_labels.rows - 1; ++v)
    {
        for (int u = 1; u < cell_map_labels.cols - 1; ++u)
        {
            // 只检查邻域关系的边界点
            // only check the border points for neighborhood relationships
            if (cell_map_labels.at<int>(v, u) == -1)
            {
                const int label_left = cell_map_labels.at<int>(v, u - 1);
                const int label_right = cell_map_labels.at<int>(v, u + 1);
                if (label_left > 0 && label_right > 0)
                {
                    cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]);
                    cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]);
                }
                const int label_up = cell_map_labels.at<int>(v - 1, u);
                const int label_down = cell_map_labels.at<int>(v + 1, u);
                if (label_up > 0 && label_down > 0)
                {
                    cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]);
                    cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]);
                }
            }
        }
    }
#ifdef DEBUG_VISUALIZATION
    printCells(cell_index_mapping);
    cv::imshow("cell_map", cell_map);
//    cv::waitKey();
#endif
    // iteratively merge cells 迭代合并单元格
    mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width);
    // re-assign area labels to 1,2,3,4,... 重新分配区域标签到1,2,3,4，…
    int new_cell_label = 1;
    for (auto itc = cell_index_mapping.begin(); itc != cell_index_mapping.end(); ++itc, ++new_cell_label)
    {
        for (int v = 0; v < cell_map_labels.rows; ++v)
        {
            for (int u = 0; u < cell_map_labels.cols; ++u)
            {
                if (cell_map_labels.at<int>(v, u) == itc->second->label_)
                    cell_map_labels.at<int>(v, u) = new_cell_label;
            }
        }
    }
    std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size()
              << " cells remaining after merging." << std::endl;
    return cell_index_mapping.size();
}

// 当看到有label_跟area_的时候，有过opencv图像处理经验的人可能就已经想到ROS是使用水漫法来做区间分离了。
// ROS里面给出的代码实现也比较简单，就不过多展开描述了，
// 主要关键的地方了解两个就行：
// 第一是使用了multimap这种数据结构来保存面积与单元相邻区间的信息area_to_region_id_mapping，key为面积，value为区间；
// 第二是合并的一个原则是将面积小于最小面积或过窄的区间合并到相邻面积最大的区间。
void BoustrophedonExplorer::mergeCellsSelection(cv::Mat &cell_map, cv::Mat &cell_map_labels,
                                                std::map<int, boost::shared_ptr<BoustrophedonCell> > &cell_index_mapping,
                                                const double min_cell_area, const int min_cell_width)
{
    // iteratively merge cells
    // merge small cells below min_cell_area with their largest neighboring cell
    // maps the area of each cell --> to the respective cell
    // 迭代合并单元格 合并min_cell_area以下的小单元格与其最大的相邻单元格 将每个单元格的面积——>映射到各自的单元格
    std::multimap<double, boost::shared_ptr<BoustrophedonCell> > area_to_region_id_mapping;
    for (auto &itc : cell_index_mapping)
    {
        area_to_region_id_mapping.insert(
                std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc.second->area_, itc.second));
    }
    for (auto it = area_to_region_id_mapping.begin(); it != area_to_region_id_mapping.end();)
    {
        // skip if segment is large enough (area and side length criteria) 如果分段足够大(面积和边长标准)，跳过
        if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width &&
            it->second->bounding_box_.height >= min_cell_width)
        {
            ++it;
            continue;
        }
        // skip segments which have no neighbors 跳过没有邻居的段
        if (it->second->neighbors_.empty())
        {
            std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
            ++it;
            continue;
        }
        // determine the largest neighboring cell 确定最大的相邻单元格
        const BoustrophedonCell &small_cell = *(it->second);
        std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double> > area_sorted_neighbors;
        for (const auto &neighbor : small_cell.neighbors_)
            area_sorted_neighbors.insert(
                    std::pair<double, boost::shared_ptr<BoustrophedonCell> >(neighbor->area_, neighbor));
        BoustrophedonCell &large_cell = *(area_sorted_neighbors.begin()->second);
        // merge the cells 合并单元格
        mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);
        // update area_to_region_id_mapping
        area_to_region_id_mapping.clear();
        for (auto &itc : cell_index_mapping)
        {
            area_to_region_id_mapping.insert(
                    std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc.second->area_, itc.second));
        }
        it = area_to_region_id_mapping.begin();
#ifdef DEBUG_VISUALIZATION
        //        printCells(cell_index_mapping);
        //        cv::imshow("cell_map", cell_map);
        //        cv::waitKey();
#endif
    }
    // label remaining border pixels with label of largest neighboring region label
    // 用最大相邻区域标签标记剩余边界像素
    for (int v = 1; v < cell_map.rows - 1; ++v)
    {
        for (int u = 1; u < cell_map.cols - 1; ++u)
        {
            if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE)
            {
                std::set<int> neighbor_labels;
                for (int dv = -1; dv <= 1; ++dv)
                {
                    for (int du = -1; du <= 1; ++du)
                    {
                        const int &val = cell_map_labels.at<int>(v + dv, u + du);
                        if (val > 0) neighbor_labels.insert(val);
                    }
                }
                if (!neighbor_labels.empty())
                {
                    int new_label = -1;
                    for (auto it = area_to_region_id_mapping.rbegin();
                         it != area_to_region_id_mapping.rend(); ++it)
                    {
                        if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
                        {
                            cell_map_labels.at<int>(v, u) = it->second->label_;
                            break;
                        }
                    }
                }
                else std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors."
                               << std::endl;
            }
        }
    }
}

void BoustrophedonExplorer::mergeTwoCells(
        cv::Mat &cell_map, cv::Mat &cell_map_labels, const BoustrophedonCell &minor_cell,
        BoustrophedonCell &major_cell, std::map<int, boost::shared_ptr<BoustrophedonCell> > &cell_index_mapping)
{
    // execute merging the minor cell into the major cell --> remove border from maps
    // 执行合并小单元格到主单元格 ——> 从地图中移除边界
    for (int v = 0; v < cell_map.rows; ++v)
    {
        for (int u = 0; u < cell_map.cols; ++u)
        {
            if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE &&
                ((cell_map_labels.at<int>(v, u - 1) == minor_cell.label_ && cell_map_labels.at<int>(v, u + 1) == major_cell.label_) ||
                 (cell_map_labels.at<int>(v, u - 1) == major_cell.label_ && cell_map_labels.at<int>(v, u + 1) == minor_cell.label_) ||
                 (cell_map_labels.at<int>(v - 1, u) == minor_cell.label_ && cell_map_labels.at<int>(v + 1, u) == major_cell.label_) ||
                 (cell_map_labels.at<int>(v - 1, u) == major_cell.label_ && cell_map_labels.at<int>(v + 1, u) == minor_cell.label_)))
            {
                cell_map.at<uchar>(v, u) = 255;
                cell_map_labels.at<int>(v, u) = major_cell.label_;
                major_cell.area_ += 1;
            }
        }
    }
    //   --> update old label in cell_map_labels 更新cell_map_labels中的旧标签
    for (int v = 0; v < cell_map_labels.rows; ++v)
    {
        for (int u = 0; u < cell_map_labels.cols; ++u)
        {
            if (cell_map_labels.at<int>(v, u) == minor_cell.label_)
                cell_map_labels.at<int>(v, u) = major_cell.label_;
        }
    }
    //   --> update major_cell 更新major_cell
    major_cell.area_ += minor_cell.area_;
    for (auto itn = major_cell.neighbors_.begin(); itn != major_cell.neighbors_.end(); ++itn)
    {
        if ((*itn)->label_ == minor_cell.label_)
        {
            major_cell.neighbors_.erase(itn);
            break;
        }
    }
    for (const auto &neighbor : minor_cell.neighbors_)
    {
        if (neighbor->label_ != major_cell.label_)
            major_cell.neighbors_.insert(neighbor);
    }

    // clean all references to minor_cell 清除对minor_cell的所有引用
    cell_index_mapping.erase(minor_cell.label_);
    for (auto &itc : cell_index_mapping)
    {
        for (auto itn = itc.second->neighbors_.begin(); itn != itc.second->neighbors_.end(); ++itn)
        {
            if ((*itn)->label_ == minor_cell.label_)
            {
                (*itn)->label_ = major_cell.label_;
                break;
            }
        }
    }
}

void BoustrophedonExplorer::correctThinWalls(cv::Mat &room_map)
{
    for (int v = 1; v < room_map.rows; ++v)
    {
        for (int u = 1; u < room_map.cols; ++u)
        {
            if (room_map.at<uchar>(v - 1, u - 1) == 255 && room_map.at<uchar>(v - 1, u) == 0 &&
                room_map.at<uchar>(v, u - 1) == 0 && room_map.at<uchar>(v, u) == 255)
                room_map.at<uchar>(v, u) = 0;
            else if (room_map.at<uchar>(v - 1, u - 1) == 0 && room_map.at<uchar>(v - 1, u) == 255 &&
                     room_map.at<uchar>(v, u - 1) == 255 && room_map.at<uchar>(v, u) == 0)
                room_map.at<uchar>(v, u - 1) = 0;
        }
    }
}
/**
 * 对每一个区间生成弓字形覆盖路径前，会对每一个区间计算最佳旋转角度，并在进行区间分割后，从图上依次获取每一个子区间，
 * 旋转到最佳角度（可简单理解为该角度下生成的覆盖直线最长，数量最小）。
 * 生成弓字形覆盖路径的过程分为两部分：
 * （1）从上到下生成直线覆盖路径，每一个区间都会生成一或多条覆盖线。
 * （2）根据当前点到区间四个顶点的距离长短，选用距离当前点最近的顶点作为弓字形路径起始点，连接当前覆盖直线最后一个路径点与下一条。
 * 其中，
 * （1）中生成的多条覆盖直线是一开始根据机器人轮廓计算得出的，
 * （2）中在生成一个区间内的覆盖直线点后，会取最后一个点作为当前点计算与该点距离最近的区间。
 * 需要注意的是robot_pos是指当前点，在每一次执行完都会修改该点数据。
 */
void BoustrophedonExplorer::computeBoustrophedonPath(const cv::Mat &room_map, const float map_resolution,
                                                     const GeneralizedPolygon &cell,
                                                     std::vector<cv::Point2f> &fov_middlepoint_path,
                                                     cv::Point &robot_pos,
                                                     const int grid_spacing_as_int, const int half_grid_spacing_as_int,
                                                     const double path_eps, const int max_deviation_from_track,
                                                     const int grid_obstacle_offset)
{
    // get a map that has only the current cell drawn in 获取只包含当前单元格的地图
    // Remark: single cells are obstacle free so it is sufficient to use the cell to check if a position can be reached during the execution of the coverage path
    cv::Mat cell_map;
    cell.drawPolygon(cell_map, cv::Scalar(255));
    // align the longer dimension of the cell horizontally with the x-axis
    cv::Point cell_center = cell.getBoundingBoxCenter();
    cv::Mat R_cell;
    cv::Rect cell_bbox;
    cv::Mat rotated_cell_map;
    RoomRotator cell_rotation;
    cell_rotation.computeRoomRotationMatrix(cell_map, R_cell, cell_bbox, map_resolution, &cell_center);
    cell_rotation.rotateRoom(cell_map, rotated_cell_map, R_cell, cell_bbox);
    // create inflated obstacles room map and rotate according to cell
    //  --> used later for checking accessibility of Boustrophedon path inside the cell
    cv::Mat inflated_room_map, rotated_inflated_room_map;
    cv::erode(room_map, inflated_room_map, cv::Mat(), cv::Point(-1, -1),
              half_grid_spacing_as_int + grid_obstacle_offset);
    cell_rotation.rotateRoom(inflated_room_map, rotated_inflated_room_map, R_cell, cell_bbox);
    cv::Mat rotated_inflated_cell_map = rotated_cell_map.clone();
    for (int v = 0; v < rotated_inflated_cell_map.rows; ++v)
        for (int u = 0; u < rotated_inflated_cell_map.cols; ++u)
            if (rotated_inflated_cell_map.at<uchar>(v, u) != 0 && rotated_inflated_room_map.at<uchar>(v, u) == 0)
                rotated_inflated_cell_map.at<uchar>(v, u) = 128;
#ifdef DEBUG_VISUALIZATION
    cv::imshow("rotated_cell_map_with_inflation", rotated_inflated_cell_map);
#endif
    // this was deactivated because it is not as accurate as the direct check within GridGenerator::generateBoustrophedonGrid,
    // because the rotation introduces some rounding errors
//	// get the min/max x/y values for this cell
//	int min_x=100000, max_x=0, min_y=100000, max_y=0;
//	std::vector<cv::Point> rotated_vertexes = cell.getVertices();
//	cv::transform(rotated_vertexes, rotated_vertexes, R_cell);
//	for(size_t point=0; point<rotated_vertexes.size(); ++point)
//	{
//		if(rotated_vertexes[point].x > max_x)
//			max_x = rotated_vertexes[point].x;
//		if(rotated_vertexes[point].y > max_y)
//			max_y = rotated_vertexes[point].y;
//		if(rotated_vertexes[point].x < min_x)
//			min_x = rotated_vertexes[point].x;
//		if(rotated_vertexes[point].y < min_y)
//			min_y = rotated_vertexes[point].y;
//	}
    // compute the basic Boustrophedon grid lines
    // step1. 计算得出所有连续的路径点并保存 Boustrophedon网格线
    BoustrophedonGrid grid_lines;
    GridGenerator::generateBoustrophedonGrid(rotated_cell_map, rotated_inflated_cell_map, -1, grid_lines,
                                             cv::Vec4i(-1, -1, -1, -1), //cv::Vec4i(min_x, max_x, min_y, max_y),
                                             grid_spacing_as_int, half_grid_spacing_as_int, 1,
                                             max_deviation_from_track);
#ifdef DEBUG_VISUALIZATION
    cv::Mat rotated_cell_map_disp = rotated_cell_map.clone();
    for (size_t i=0; i<grid_lines.size(); ++i)
    {
        for (size_t j=0; j+1<grid_lines[i].upper_line.size(); ++j)
        {
            cv::circle(rotated_cell_map_disp, grid_lines[i].upper_line[j], 1, cv::Scalar(64), cv::FILLED);
            cv::line(rotated_cell_map_disp, grid_lines[i].upper_line[j], grid_lines[i].upper_line[j+1], cv::Scalar(128), 1);
        }
        for (size_t j=0; j+1<grid_lines[i].lower_line.size(); ++j)
        {
            cv::circle(rotated_cell_map_disp, grid_lines[i].lower_line[j], 1, cv::Scalar(64), cv::FILLED);
            cv::line(rotated_cell_map_disp, grid_lines[i].lower_line[j], grid_lines[i].lower_line[j+1], cv::Scalar(196), 1);
        }
    }
    cv::imshow("rotated_cell_map", rotated_cell_map_disp);
#endif
    // if no edge could be found in the cell (e.g. if it is too small), ignore it
    if (grid_lines.empty()) return;
    // get the edge nearest to the current robot position to start the boustrophedon path at, by looking at the
    // upper and lower horizontal path (possible nearest locations) for the edges transformed to the original coordinates (easier)
    std::vector<cv::Point> outer_corners(4);
    outer_corners[0] = grid_lines[0].upper_line[0];          // upper left corner
    outer_corners[1] = grid_lines[0].upper_line.back();      // upper right corner
    outer_corners[2] = grid_lines.back().upper_line[0];      // lower left corner
    outer_corners[3] = grid_lines.back().upper_line.back();  // lower right corner
    cv::Mat R_cell_inv;
    // invert the rotation matrix to remap the determined points to the original cell
    // 反转旋转矩阵以将确定的点重新映射到原始单元格
    cv::invertAffineTransform(R_cell, R_cell_inv);
    cv::transform(outer_corners, outer_corners, R_cell_inv);
    double min_corner_dist = path_planner_.planPath(room_map, robot_pos, outer_corners[0], 1.0, 0.0, map_resolution);
    int min_corner_index = 0;
    for (int i = 1; i < 4; ++i)
    {
        double dist = path_planner_.planPath(room_map, robot_pos, outer_corners[i], 1.0, 0.0, map_resolution);
        if (dist < min_corner_dist)
        {
            min_corner_dist = dist;
            min_corner_index = i;
        }
    }
    bool start_from_upper_path = min_corner_index < 2;
    // boolean to determine on which side the path should start and to check where the path ended
    auto start_from_left = min_corner_index % 2 == 0;
#ifdef DEBUG_VISUALIZATION
    cv::Mat room_map_disp = room_map.clone();
    for (size_t i=0; i<outer_corners.size(); i+=2)
        cv::line(room_map_disp, outer_corners[i], outer_corners[i+1], cv::Scalar(128), 1);
    cv::circle(room_map_disp, robot_pos, 3, cv::Scalar(160), cv::FILLED);
    if (start_from_upper_path)
    {
        if (start_from_left == true)
            cv::circle(room_map_disp, outer_corners[0], 3, cv::Scalar(64), cv::FILLED);
        else
            cv::circle(room_map_disp, outer_corners[1], 3, cv::Scalar(64), cv::FILLED);
    }
    else
    {
        if (start_from_left == true)
            cv::circle(room_map_disp, outer_corners[2], 3, cv::Scalar(64), cv::FILLED);
        else
            cv::circle(room_map_disp, outer_corners[3], 3, cv::Scalar(64), cv::FILLED);
    }
    cv::imshow("rotated_room_map", room_map_disp);
#endif
    // connect the boustrophedon paths
    // 连接boustrophedon路径
    cv::Point cell_robot_pos;
    bool start = true;
    std::vector<cv::Point> current_fov_path;
    // plan the path starting from upper horizontal line
    // 从上水平线开始规划路径
    // step2. 生成连续覆盖直线后，执行第二个步骤：按照给定的点间距取点并按照弓字形连接生成路径点。
    // downsamplePath是从左往右取点，downsamplePathReverse是从右往左取点，
    // path_eps是路径点的间隔，即第一条直线从左往右取点，下一条直线从右往左取点，再下一条从左往右取点，这样依次循环执行，就生成弓字形的覆盖路径点。
    if (start_from_upper_path)
    {
        for (auto &grid_line : grid_lines)
        {
            // at the beginning of path planning start at first horizontal line --> no vertical points between lines
            // 在路径规划开始时，从第一条水平线开始——>线与线之间没有垂直点
            if (start)
            {
                if (start_from_left) cell_robot_pos = grid_line.upper_line[0];
                else cell_robot_pos = grid_line.upper_line.back();
                start = false;
            }
            // plan path from left to right corner
            // 从左到右规划路径
            if (start_from_left)
            {
                // get points on transition between horizontal lines by using the Astar-path
                // 使用A*在水平线之间获取过渡点
                std::vector<cv::Point> astar_path;
                path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, grid_line.upper_line[0], 1.0, 0.0,
                                       map_resolution, 0, &astar_path);
                downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);
                // get points between left and right corner 获取左右角之间的点数
                downsamplePath(grid_line.upper_line, current_fov_path, cell_robot_pos, path_eps);
                // add the lower path of the current line if available (and then start from the left side again next time)
                // 如果可用，添加当前行的下路径(然后下次再从左侧开始)
                if (grid_line.has_two_valid_lines)
                    downsamplePathReverse(grid_line.lower_line, current_fov_path, cell_robot_pos, path_eps);
                    // start from the right side next time  下次从右侧开始
                else start_from_left = false;

            }
                // plan path from right to left corner
            else
            {
                // get points on transition between horizontal lines by using the Astar-path
                std::vector<cv::Point> astar_path;
                path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, grid_line.upper_line.back(), 1.0, 0.0,
                                       map_resolution, 0, &astar_path);
                downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);
                // get points between right and left corner
                downsamplePathReverse(grid_line.upper_line, current_fov_path, cell_robot_pos, path_eps);
                // add the lower path of the current line if available (and then start from the right side again next time)
                if (grid_line.has_two_valid_lines)
                    downsamplePath(grid_line.lower_line, current_fov_path, cell_robot_pos, path_eps);
                    // start from the left side next time
                else start_from_left = true;
            }
        }
    }
        // plan the path from the lower horizontal line
    else
    {
        for (auto line = grid_lines.rbegin(); line != grid_lines.rend(); ++line)
        {
            // at the beginning of path planning start at first horizontal line --> no vertical points between lines
            if (start)
            {
                if (start_from_left) cell_robot_pos = line->upper_line[0];
                else cell_robot_pos = line->upper_line.back();
                start = false;
            }
            // plan path from left to right corner
            if (start_from_left)
            {
                // get points on transition between horizontal lines by using the Astar-path
                std::vector<cv::Point> astar_path;
                path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line[0], 1.0, 0.0,
                                       map_resolution, 0, &astar_path);
                downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);
                // get points between left and right corner
                downsamplePath(line->upper_line, current_fov_path, cell_robot_pos, path_eps);
                // add the lower path of the current line if available (and then start from the left side again next time)
                if (line->has_two_valid_lines)
                    downsamplePathReverse(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
                    // start from the right side next time
                else start_from_left = false;
            }
                // plan path from right to left corner 从右到左规划路径
            else
            {
                // get points on transition between horizontal lines by using the Astar-path
                // 利用A*在水平线之间得到过渡点
                std::vector<cv::Point> astar_path;
                path_planner_.planPath(rotated_inflated_cell_map, cell_robot_pos, line->upper_line.back(), 1.0, 0.0,
                                       map_resolution, 0, &astar_path);
                downsamplePath(astar_path, current_fov_path, cell_robot_pos, path_eps);
                // get points between right and left corner 获得左右角之间的点
                downsamplePathReverse(line->upper_line, current_fov_path, cell_robot_pos, path_eps);
                // add the lower path of the current line if available (and then start from the right side again next time)
                if (line->has_two_valid_lines)
                    downsamplePath(line->lower_line, current_fov_path, cell_robot_pos, path_eps);
                    // start from the left side next time
                else start_from_left = true;
            }
        }
    }
#ifdef DEBUG_VISUALIZATION
    cv::Mat rotated_cell_fov_path_disp = rotated_cell_map.clone();
    for (size_t i=1; i<current_fov_path.size(); ++i)
    {
        cv::circle(rotated_cell_fov_path_disp, current_fov_path[i], 1, cv::Scalar(196), 1);
        cv::line(rotated_cell_fov_path_disp, current_fov_path[i-1], current_fov_path[i], cv::Scalar(128), 1);
    }
    cv::imshow("rotated_cell_fov_path", rotated_cell_fov_path_disp);
#endif
    // remap the fov path to the originally rotated cell and add the found points to the global path
    std::vector<cv::Point2f> fov_middlepoint_path_part;
    for (auto &point : current_fov_path)
        fov_middlepoint_path_part.emplace_back(point.x, point.y);
    cv::transform(fov_middlepoint_path_part, fov_middlepoint_path_part, R_cell_inv);
    fov_middlepoint_path.insert(fov_middlepoint_path.end(), fov_middlepoint_path_part.begin(),
                                fov_middlepoint_path_part.end());
#ifdef DEBUG_VISUALIZATION
    cv::Mat cell_fov_path_disp = cell_map.clone();
    for (size_t i=1; i<fov_middlepoint_path.size(); ++i)
    {
        cv::circle(cell_fov_path_disp, fov_middlepoint_path[i], 1, cv::Scalar(196), 1);
        cv::line(cell_fov_path_disp, fov_middlepoint_path[i-1], fov_middlepoint_path[i], cv::Scalar(128), 1);
    }
    cv::imshow("cell_fov_path", cell_fov_path_disp);
    cv::waitKey();
#endif
    // also update the current robot position  也更新当前机器人的位置
    std::vector<cv::Point> current_pos_vector(1, cell_robot_pos);
    cv::transform(current_pos_vector, current_pos_vector, R_cell_inv);
    robot_pos = current_pos_vector[0];
}

void BoustrophedonExplorer::downsamplePath(const std::vector<cv::Point> &original_path,
                                           std::vector<cv::Point> &downsampled_path,
                                           cv::Point &robot_pos, const double path_eps)
{
    // downsample path
    for (const auto &path_point : original_path)
    {
        if (cv::norm(robot_pos - path_point) >= path_eps)
        {
            downsampled_path.push_back(path_point);
            robot_pos = path_point;
        }
    }
    // add last element
    if (!original_path.empty())
    {
        downsampled_path.push_back(original_path.back());
        robot_pos = original_path.back();
    }
}

void BoustrophedonExplorer::downsamplePathReverse(const std::vector<cv::Point> &original_path,
                                                  std::vector<cv::Point> &downsampled_path,
                                                  cv::Point &robot_pos, const double path_eps)
{
    // downsample path
    for (size_t path_point = original_path.size() - 1;; --path_point)
    {
        if (cv::norm(robot_pos - original_path[path_point]) >= path_eps)
        {
            downsampled_path.push_back(original_path[path_point]);
            robot_pos = original_path[path_point];
        }
        if (path_point == 0)
            break;
    }
    // add last element
    if (!original_path.empty())
    {
        downsampled_path.push_back(original_path[0]);
        robot_pos = original_path[0];
    }
}

void BoustrophedonExplorer::printCells(std::map<int, boost::shared_ptr<BoustrophedonCell> > &cell_index_mapping)
{
    std::cout << "---\n";
    for (auto &itc : cell_index_mapping)
    {
        std::cout << itc.first << ": l=" << itc.second->label_ << "   a=" << itc.second->area_ << "   n=";
        for (auto its = itc.second->neighbors_.begin(); its != itc.second->neighbors_.end(); ++its)
            std::cout << (*its)->label_ << ", ";
        std::cout << std::endl;
    }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// BoustrophedonVariantExplorer
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void BoustrophedonVariantExplorer::mergeCellsSelection(cv::Mat &cell_map, cv::Mat &cell_map_labels,
                                                       std::map<int, boost::shared_ptr<BoustrophedonCell> > &cell_index_mapping,
                                                       const double min_cell_area, const int min_cell_width)
{
    // iteratively merge cells
    //todo:
    // - take one major cell (the largest) and its major direction
    // - merge every other cell into the major cell, except
    //   - the width along the major direction is too small and the cell is sufficiently large
    //   - the bounding box orientation (side length ratio) deviates strongly from the major direction
    //   - the cell main direction is not well aligned with the major direction (skew, 90 deg)
    RoomRotator room_rotator;
    //double rotation_angle = room_rotator.computeRoomMainDirection(cell_map, map_resolution);
    // merge small cells below min_cell_area with their largest neighboring cell
    // maps the area of each cell --> to the respective cell
    std::multimap<double, boost::shared_ptr<BoustrophedonCell> > area_to_region_id_mapping;
    for (auto &itc : cell_index_mapping)
        area_to_region_id_mapping.insert(
                std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc.second->area_, itc.second));
    for (auto it = area_to_region_id_mapping.begin(); it != area_to_region_id_mapping.end();)
    {
        // abort if no cells below min_cell_area remain unmerged into bigger cells
        if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width &&
            it->second->bounding_box_.height >= min_cell_width)
        {
            ++it;
            continue;
        }
        // skip segments which have no neighbors
        if (it->second->neighbors_.size() == 0)
        {
            std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
            ++it;
            continue;
        }
        // determine the largest neighboring cell
        const BoustrophedonCell &small_cell = *(it->second);
        std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double> > area_sorted_neighbors;
        for (const auto &neighbor : small_cell.neighbors_)
            area_sorted_neighbors.insert(
                    std::pair<double, boost::shared_ptr<BoustrophedonCell> >(neighbor->area_, neighbor));
        BoustrophedonCell &large_cell = *(area_sorted_neighbors.begin()->second);
        // merge the cells
        mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);
        // update area_to_region_id_mapping
        area_to_region_id_mapping.clear();
        for (auto &itc : cell_index_mapping)
            area_to_region_id_mapping.insert(
                    std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc.second->area_, itc.second));
        it = area_to_region_id_mapping.begin();

#ifdef DEBUG_VISUALIZATION
        printCells(cell_index_mapping);
        cv::imshow("cell_map", cell_map);
        cv::waitKey();
#endif
    }

    // label remaining border pixels with label of largest neighboring region label
    for (int v = 1; v < cell_map.rows - 1; ++v)
    {
        for (int u = 1; u < cell_map.cols - 1; ++u)
        {
            if (cell_map.at<uchar>(v, u) == BORDER_PIXEL_VALUE)
            {
                std::set<int> neighbor_labels;
                for (int dv = -1; dv <= 1; ++dv)
                {
                    for (int du = -1; du <= 1; ++du)
                    {
                        const int &val = cell_map_labels.at<int>(v + dv, u + du);
                        if (val > 0) neighbor_labels.insert(val);
                    }
                }
                if (!neighbor_labels.empty())
                {
                    int new_label = -1;
                    for (auto it = area_to_region_id_mapping.rbegin(); it != area_to_region_id_mapping.rend(); ++it)
                    {
                        if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
                        {
                            cell_map_labels.at<int>(v, u) = it->second->label_;
                            break;
                        }
                    }
                }
                else std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors."
                              << std::endl;
            }
        }
    }
}

//void BoustrophedonVariantExplorer::computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
//		std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
//{
//	std::cout << "Calling BoustrophedonVariantExplorer::computeCellDecomposition..." << std::endl;
//
//	// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
//	// create a map copy to mark the cell boundaries
//	cv::Mat cell_map = room_map.clone();
//#ifdef DEBUG_VISUALIZATION
//	cv::imshow("cell_map", cell_map);
//#endif
//
//
//	// *********************** III. Find the separated cells. ***********************
//	std::vector<std::vector<cv::Point> > cells;
//	cv::Mat cell_copy = cell_map.clone();
//	correctThinWalls(cell_copy);	// just adds a few obstacle pixels to avoid merging independent segments
//	cv::findContours(cell_copy, cells, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
//#ifdef DEBUG_VISUALIZATION
////	 testing
////	cv::Mat black_map = cv::Mat(cell_map.rows, cell_map.cols, cell_map.type(), cv::Scalar(0));
////	for(size_t i=0; i<cells.size(); ++i)
////	{
////		cv::drawContours(black_map, cells, i, cv::Scalar(127), cv::FILLED);
////		cv::imshow("contours", black_map);
////		cv::waitKey();
////	}
//#endif
//
//	// create generalized Polygons out of the contours to handle the cells
//	for(size_t cell=0; cell<cells.size(); ++cell)
//	{
//		if(cv::contourArea(cells[cell])>=min_cell_area)
//		{
//			GeneralizedPolygon current_cell(cells[cell], map_resolution);
//			cell_polygons.push_back(current_cell);
//			polygon_centers.push_back(current_cell.getCenter());
//		}
//	}
//}
